import numpy as np

def gen_A(camera_coordinate):
    A = []
    for p in camera_coordinate:
        A.append([p[0], p[1], 1, 0])
        A.append([p[1], -p[0], 0, 1])
    return A

def gen_b(base_coordinate):
    b = []
    for p in base_coordinate:
        b.append(p[0])
        b.append(p[1])

    return b

def cvt(p_camera, x):
    p1 = p_camera[0]*x[0] + p_camera[1]*x[1] + x[2]
    p2 = p_camera[1]*x[0] - p_camera[0]*x[1] + x[3]

    return [p1, p2]



if __name__ == "__main__":
    # 求解
    yz = [[42.1122, 290.308],
    [41.7609, 265.89],
    [52.3697,294.123],
    [27.021908, 290.10501],
    [42.463207, 297.70395],
    [52.459576, 266.10068],
    [21.821011, 274.561],
    [44.288586, 264.89307],
    [34.749512, 294.45355],
    [-16.066031, 306.85333],
    [6.8973093, 306.53073],
    [29.004229, 291.66971],
    [47.223293, 312.50272],
    [53.4366, 272.59015],
    [52.889908, 255.17247]]
    
    camera_coordinate = [[-42.1122, 290.308],
    [-41.7609, 265.89],
    [-52.3697,294.123],
    [-27.021908, 290.10501],
    [-42.463207, 297.70395],
    [-52.459576, 266.10068],
    [-21.821011, 274.561],
    [-44.288586, 264.89307],
    [-34.749512, 294.45355],
    [16.066031, 306.85333],
    [-6.8973093, 306.53073],
    [-29.004229, 291.66971],
    [-47.223293, 312.50272],
    [-53.4366, 272.59015],
    [-52.889908, 255.17247]]

    xy = [[54.28473, 87.73456],
    [84.28458, 88.00456],
    [55.03472, 83.48958],
    [54.479729, 87.029564],
    [67.229668, 74.279625],
    [115.42442, 78.764603],
    [60.674698, 103.51448],
    [92.924538, 92.264542],
    [64.424675, 84.01458],
    [5.9249701, 96.854515],
    [29.174854, 90.854546],
    [60.224697, 89.354553],
    [65.444672, 66.314667],
    [93.85453, 75.314621],
    [102.85448, 82.124588]]
    base_coordinate = xy

    A_list = gen_A(camera_coordinate)
    b_list = gen_b(base_coordinate)

    A = np.array(A_list)
    b = np.array(b_list)
    # print(A)
    # print(b)

    # 计算伪逆
    pinv_A = np.linalg.pinv(A)

    # 计算x值
    x = np.dot(pinv_A, b)

    M = np.array([[x[0], x[1], x[2]],
                  [-x[1], x[0], x[3]],
                  [0, 0, 1]])
    # print(M)
    # 三个对应点
    # [[ 1.00193625e-01 -1.01306515e+00  3.56228421e+02]
    # [ 1.01306515e+00  1.00193625e-01  1.04018263e+02]
    # [ 0.00000000e+00  0.00000000e+00  1.00000000e+00]]
    for c in camera_coordinate:
        print(cvt(c,x))

    # print(M)
